import cv2
import numpy as np
import rospy
from collections import deque

from object_detect.msg import detect

# node initialize
rospy.init_node('color_shape_detect', anonymous=True)
pub = rospy.Publisher('Detect_Output', detect, queue_size=10)
# for keeping center points of object
buffer_size = 16
pts = deque(maxlen=buffer_size)

# blue HSV
# blueLower = (84,  98,  0)
# blueUpper = (179, 255, 255)

# red HSV
lowerred_0 = np.array([0,100,100])
upperred_0 = np.array([10, 255, 255])
lowerred_1 = np.array([170,100,100])
upperred_1 = np.array([180, 255, 255])

#capture
cap = cv2.VideoCapture(0)
cap.set(3,960) #set width of capture
cap.set(4,480) #set height of capture

while True:

    success, imgOriginal = cap.read()

    if success:

        #blur
        blurred = cv2.GaussianBlur(imgOriginal, (11,11), 0)

        # HSV
        hsv = cv2.cvtColor(blurred, cv2.COLOR_BGR2HSV)
        cv2.imshow("HSV IMAGE", hsv)
        
        # # original image to gray
        # im = cv2.imread(imgOriginal, cv2.IMREAD_GRAYSCALE)

        # mask for red
        # mask = cv2.inRange(hsv, lowerred, upperred)
        mask_0 = cv2.inRange(hsv, lowerred_0, upperred_0)
        mask_1 = cv2.inRange(hsv, lowerred_1, upperred_1)
        mask = cv2.bitwise_or(mask_0, mask_1) #OR per bit 

        # deleting noises which are in area of mask
        mask = cv2.erode(mask, None, iterations=2)
        mask = cv2.dilate(mask, None, iterations=2)
        cv2.imshow("Mask + Erosion + Dilation", mask)

        # contours
        img,contours,_ = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
        center = None

        if len(contours) > 0:

            # get max contour
            c = max(contours, key=cv2.contourArea)

            # add new detection about shape
           
            for cnt in range(len(contours)):
                # contour approximation
                epsilon = 0.01 * cv2.arcLength(contours[cnt], True)
                approx = cv2.approxPolyDP(contours[cnt], epsilon, True)

            # analyse the shape
            corners = len(approx)
            # print("corners: %d"%corners)
            
            # circle's corners bigger than 10 
            if corners >= 10:
                # return rectangle
                rect = cv2.minAreaRect(c)
                ((x,y), (width, height), rotation) = rect #obtain rectangle's 4 corners coordinates and center point ...
                
                s = "x: %d, y: %d, width: %d, height: %d, rotation: %d"%(np.round(x), np.round(y), np.round(width), np.round(height), np.round(rotation))
                # print(s)
                
                
                # topic publish initialize
                c_xy = detect()
                c_xy.c_x = np.int64(x)
                c_xy.c_y = np.int64(y)
                # print(c_x, c_y)

                # box
                box = cv2.boxPoints(rect) #obtain rectangle's 4 corners' coordinates (float)
                box = np.int64(box) #from float to int
    
                # moment---to obtain the center coordinate 
                M = cv2.moments(c)
                center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
    
                # draw contour
                cv2.drawContours(imgOriginal, [box], 0, (0, 255, 255), 2)
    
                # point in center----draw a small circle, and in this part, it will show a point on picture to show the center of rectangle
                cv2.circle(imgOriginal, center, 5, (255, 0, 255), -1)
    
                # print inform
                cv2.putText(imgOriginal, s, (25, 50), cv2.FONT_HERSHEY_COMPLEX_SMALL, 1, (255, 255, 255), 2)


                pub.publish(c_xy)
                rospy.loginfo("publish coordinate massage:x=%d,y=%d" %(c_xy.c_x, c_xy.c_y))
                

        # deque
        pts.appendleft(center)
        for i in range(1, len(pts)):

            if pts[i - 1] is None or pts[i] is None: continue

            cv2.line(imgOriginal, pts[i - 1], pts[i], (0, 255, 0), 3)

        cv2.imshow("DETECTED IMAGE", imgOriginal)
        
        # pub.publish(detect.c_x, detect.c_y)
        # rospy.loginfo("publish coordinate massage:x=%d,y=%d" %(detect.c_x, detect.c_y))

    if cv2.waitKey(1) & 0xFF == ord("q"): break



